<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package frc3128.HardwareLink.Motor.SpeedControl;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import frc3128.HardwareLink.Motor.MotorSpeedControl;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import frc3128.Util.RobotMath;<a name="line.4"></a>
<span class="sourceLineNo">005</span><a name="line.5"></a>
<span class="sourceLineNo">006</span>/**<a name="line.6"></a>
<span class="sourceLineNo">007</span> * Used for computing a target power with a linear shift.<a name="line.7"></a>
<span class="sourceLineNo">008</span> * <a name="line.8"></a>
<span class="sourceLineNo">009</span> * @author Noah Sutton-Smolin<a name="line.9"></a>
<span class="sourceLineNo">010</span> */<a name="line.10"></a>
<span class="sourceLineNo">011</span>//TODO Test LinearSpeedTarget<a name="line.11"></a>
<span class="sourceLineNo">012</span>public class LinearSpeedTarget extends MotorSpeedControl {<a name="line.12"></a>
<span class="sourceLineNo">013</span>    private double tgtSpeed;<a name="line.13"></a>
<span class="sourceLineNo">014</span>    private double dtAccum;<a name="line.14"></a>
<span class="sourceLineNo">015</span>    private double refreshTime = 50;<a name="line.15"></a>
<span class="sourceLineNo">016</span>    private double lastEncoderAngle = -1;<a name="line.16"></a>
<span class="sourceLineNo">017</span><a name="line.17"></a>
<span class="sourceLineNo">018</span>    /**<a name="line.18"></a>
<span class="sourceLineNo">019</span>     * <a name="line.19"></a>
<span class="sourceLineNo">020</span>     * @param tgtSpeed    target speed in deg/sec<a name="line.20"></a>
<span class="sourceLineNo">021</span>     * @param refreshTime speed update rate in msec<a name="line.21"></a>
<span class="sourceLineNo">022</span>     */<a name="line.22"></a>
<span class="sourceLineNo">023</span>    public LinearSpeedTarget(double tgtSpeed, double refreshTime) {<a name="line.23"></a>
<span class="sourceLineNo">024</span>        this.tgtSpeed = tgtSpeed; this.refreshTime = refreshTime/1000.0;<a name="line.24"></a>
<span class="sourceLineNo">025</span>        lastEncoderAngle = this.getLinkedEncoderAngle();<a name="line.25"></a>
<span class="sourceLineNo">026</span>    }<a name="line.26"></a>
<span class="sourceLineNo">027</span>    <a name="line.27"></a>
<span class="sourceLineNo">028</span>    /**<a name="line.28"></a>
<span class="sourceLineNo">029</span>     * Uses a default refreshTime of 50msec<a name="line.29"></a>
<span class="sourceLineNo">030</span>     * <a name="line.30"></a>
<span class="sourceLineNo">031</span>     * @param tgtSpeed target speed in deg/sec<a name="line.31"></a>
<span class="sourceLineNo">032</span>     */<a name="line.32"></a>
<span class="sourceLineNo">033</span>    public LinearSpeedTarget(double tgtSpeed) {this(tgtSpeed, 0.050);}<a name="line.33"></a>
<span class="sourceLineNo">034</span>    <a name="line.34"></a>
<span class="sourceLineNo">035</span>    public void setControlTarget(double d) {tgtSpeed = d; lastEncoderAngle = this.getLinkedEncoderAngle();}<a name="line.35"></a>
<span class="sourceLineNo">036</span><a name="line.36"></a>
<span class="sourceLineNo">037</span>    public double speedTimestep(double dt) {<a name="line.37"></a>
<span class="sourceLineNo">038</span>        dtAccum += dt; if(dtAccum &lt; refreshTime) return this.getLinkedMotorSpeed();<a name="line.38"></a>
<span class="sourceLineNo">039</span><a name="line.39"></a>
<span class="sourceLineNo">040</span>        // Power * (current rate of change) / (target rate of change) =&gt; pow * (deg/sec) / (deg/sec) =&gt; pow<a name="line.40"></a>
<span class="sourceLineNo">041</span>        double retVal = RobotMath.makeValidPower((this.getLinkedMotorSpeed() / tgtSpeed) * ((this.getLinkedEncoderAngle()-lastEncoderAngle)/dtAccum));<a name="line.41"></a>
<span class="sourceLineNo">042</span>        dtAccum = 0; lastEncoderAngle = this.getLinkedEncoderAngle(); return retVal;<a name="line.42"></a>
<span class="sourceLineNo">043</span>    }<a name="line.43"></a>
<span class="sourceLineNo">044</span><a name="line.44"></a>
<span class="sourceLineNo">045</span>    /**<a name="line.45"></a>
<span class="sourceLineNo">046</span>     * Sets the speed update time in msec<a name="line.46"></a>
<span class="sourceLineNo">047</span>     * <a name="line.47"></a>
<span class="sourceLineNo">048</span>     * @param refreshTime speed update rate in msec<a name="line.48"></a>
<span class="sourceLineNo">049</span>     */<a name="line.49"></a>
<span class="sourceLineNo">050</span>    public void setRefreshTime(double refreshTime) {this.refreshTime = refreshTime/1000.0;}<a name="line.50"></a>
<span class="sourceLineNo">051</span>    <a name="line.51"></a>
<span class="sourceLineNo">052</span>    public void clearControlRun() {this.tgtSpeed = 0; this.lastEncoderAngle = this.getLinkedEncoderAngle();}<a name="line.52"></a>
<span class="sourceLineNo">053</span><a name="line.53"></a>
<span class="sourceLineNo">054</span>    public boolean isComplete() {return false;}<a name="line.54"></a>
<span class="sourceLineNo">055</span>}<a name="line.55"></a>




























































</pre>
</div>
</body>
</html>
